% C \in S^{n(p+1)}
% Minimizes E(||Omega^(1/2)*w(t)||_2) - estimation error normalized

function B = dual_reg_mle(C, n, p, gamma)
    C = (1/2)*(C+C'); 
    Omega = inv(C(1:n,1:n));
    ntest = n;
	rel_err = 1;
    while(rel_err>1e-5)
        
    % Estimate A given Omega
        cvx_begin
            variable Z0(n,n) symmetric
            variable Zp(n,n*p) 
            variable S(n,n) symmetric
        % construct T
            idx = toeplitz((0:p), (0:p)');
            expression T(n*(p+1), n*(p+1));
            for i=0:p-1
                for j=i+1:p
                    T((1:n)+n*i,(1:n)+n*j) = Zp((1:n),(1:n)+n*(idx(i+1,j+1)-1));
                end
            end
            T = T + T';
            for i=0:p
                T((1:n)+n*i,(1:n)+n*i) = Z0;
            end

            % construct constraints on ZSum
            expression ZSum(n,n)
            ZSum = abs(Z0);
            for i = 1:p
                ZSum = ZSum + abs(Zp(1:n,(1:n)+n*(i-1)));
            end
            ZSum=ZSum+ZSum';

            minimize(-log_det(S));
            subject to 
                [S zeros(n,n*p); zeros(n*p,n*(p+1))] + C + T  == semidefinite(n*(p+1));
                ZSum <= gamma*(ones(n)-eye(n));
        cvx_end
        CT = C + T;
        A = [eye(n) (CT(n+1:n*(p+1),n+1:n*(p+1))\CT(n+1:n*(p+1),1:n))'];
    
    % Estimate Omega given A
        Omega_new = (A*C*A')\eye(n);
        ntest = [ntest norm(Omega-Omega_new,'fro')];
        Omega = Omega_new;

		rel_err = ntest(end)/norm(Omega,'fro');
		fprintf('CVX optimal %d, relative error ||Omega_new-Omega||_F/||Omega_new||_F %d\n',cvx_optval,rel_err);
    
    end
    B = sqrtm(Omega)*A;
end
